package org.maxgroundstation.simulation.service.video;

import org.maxgroundstation.simulation.MaxRobot;

/**
 * Simulation of landscape views requires knowledge of the cameras 
 * position, orientation, and field of view.
 * 
 * @author Judd Reed
 * 
 * Copyright (C) 2011  Judd Reed
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
public class CameraSituation {

    private MaxRobot robot;
    
    public CameraSituation(MaxRobot robot) {
        this.robot = robot;
        getCameraConfiguration();
    }

    /**
     *  called at instantiation only.
     *  Sets camera properties.
     *  DUMMIED FOR NOW.
     *  TODO: This SHOULD poll some configuration table.
     */
    private double mountX, mountY, mountZ;
    
    private void getCameraConfiguration() {
        mountX = mountY = 0.0;
        mountZ = 0.50; // camera is 50 centimeters above ground at vehicle center
    }
    
    public double[] getRobotLocation(){
        return transformGpsToUtm(
            robot.getLatitude(), robot.getLongitude(), robot.getElevation());
    }
    
    /**
     * Compute UTM Easting, Northing, and elevation based on GPS readings.
     * This should be replaced by a trusted library or built in GPS driver 
     * function. Current implementation is a brute force approximation hack.
     * 
     * Geographic to UTM coordinate conversion is a not simple.  You have to 
     * use the right earth model (spheroid and datum) and the math is a slight
     * challenge.  But you can get close enough with approximations.
     * Here I assume the earth is round and 40,000 km around. I do some very
     * simple trigonometry to scale latitude longitude into meters and adjust 
     * this result to match UTM at the Hab.  This cheat (err, I mean 'approximation')
     * should be okay for coordinates near the HAB.  By a KM or so away, errors
     * will start to show. 
     *
     * @param latitude
     * @param longitude
     * @param elevation
     * @return array with easting, northing, and elevation
     */
    private double[] transformGpsToUtm(double latitude, double longitude, double elevation) {
        final double HAB_LAT = 38.406458;
        final double HAB_LON = -110.791903; 
        final double HAB_UTM_EAST = 518169;
        final double HAB_UTM_NRTH = 4250934;
        
        double[] whereAmI = new double[3];
        
        whereAmI[0] = HAB_UTM_EAST + Math.cos(Math.PI*latitude/180.0)*(longitude-HAB_LON)*10000000.0/90.0;
        whereAmI[1] = HAB_UTM_NRTH + (latitude-HAB_LAT)*10000000.0/90.0;
        whereAmI[2] = 0.0;  // look it up in the DEM data
        return whereAmI;
    }
    
    public void informElevation(double meters){
        robot.setElevation((float)meters);   
    }
    
    /**
     * Offsets position vector from vehicle to camera
     * @param xyz[]
     * @return xyz[]
     */
    public double[] moveToCamera(double[] xyz) {
        xyz[0] += mountX;
        xyz[1] += mountY;
        xyz[2] += mountZ;
        return xyz;
    }
    
    /**
     * Computes a unit vector in the direction of camera centerline.
     * Components are ordered and signed as {east, north, up}
     * @return view direction vector
     */
    private double[] lookVector = new double[3];
    public double[] getLookingDirection() {
        /*
        double bear = robot.getTravelHeading();
        if(robot.getSpeed()<0){
            bear += Math.PI;
        }
        */
        double bear = robot.getFacing();
        bear += robot.getHeadPanAngle();
        double tilt = robot.getHeadTiltAngle();
        lookVector[2] = Math.sin(tilt);
        lookVector[1] = Math.cos(tilt) * Math.cos(bear);
        lookVector[0] = Math.cos(tilt) * Math.sin(bear);
        return lookVector;
    }
}